// Copyright 2018 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iostream>
#include <list>
#include <memory>
#include <string>

// include ROS
#ifdef __clang__
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-parameter"
#endif
#include <ros/ros.h>
#include <ros/console.h>
#ifdef __clang__
# pragma clang diagnostic pop
#endif

// include Ignition Transport
#include <ignition/transport/Node.hh>
#include <nav_msgs/Odometry.h>

// Direction of bridge.
enum Direction
{
  // Both directions.
  BIDIRECTIONAL = 0,
  // Only from IGN to ROS
  FROM_IGN_TO_ROS = 1,
  // Only from ROS to IGN
  FROM_ROS_TO_IGN = 2,
};

//////////////////////////////////////////////////
void usage()
{
  ROS_INFO_STREAM(
      "Bridge Ignition Transport topics to collection of ROS.\n\n"
      << "parameter_bridge /Ignition_topic vehicle_1  vehicle_2. "
      << std::endl);
}

struct BridgeIgnToRosHandles
{
  std::shared_ptr<ignition::transport::Node> ign_subscriber;
  ros::Publisher ros_publisher;
};

class Pose_bridge
{
  public:

      
    Pose_bridge(){};

    void convert_ign_to_ros(const ignition::msgs::Pose &ign_msg, nav_msgs::Odometry &ros_msg)
    {
      ros_msg.header.stamp = ros::Time::now();
      ros_msg.pose.pose.orientation.w = ign_msg.orientation().w();
      ros_msg.pose.pose.orientation.x = ign_msg.orientation().x();
      ros_msg.pose.pose.orientation.y = ign_msg.orientation().y();
      ros_msg.pose.pose.orientation.z = ign_msg.orientation().z();
      ros_msg.pose.pose.position.x = ign_msg.position().x();
      ros_msg.pose.pose.position.y = ign_msg.position().y();
      ros_msg.pose.pose.position.z = ign_msg.position().z();
    }

    void ign_callback(const ignition::msgs::Pose_V &ign_msg, ros::Publisher ros_pub)
    {
      auto size_ = ign_msg.pose_size();
      for (int i=0; i<size_;i++) 
      {
        std::string topic_name = ros_pub.getTopic();
        std::string vehicle_name = topic_name.substr(topic_name.rfind("/")+1, topic_name.length() - topic_name.rfind("/"));
        //std::cout<<vehicle_name<<std::endl;
        if(std::string(ign_msg.pose(i).name()) == vehicle_name)
        {
          nav_msgs::Odometry ros_msg;
          convert_ign_to_ros(ign_msg.pose(i), ros_msg);
          ros_pub.publish(ros_msg); 
        }
      }
    }
    void  create_ign_subscriber(
      std::shared_ptr<ignition::transport::Node> node,
      const std::string & topic_name,
      size_t /*queue_size*/,
      ros::Publisher ros_pub)
    {
      std::function<void(const ignition::msgs::Pose_V&,
                        const ignition::transport::MessageInfo &)> subCb =
      [this, ros_pub](const ignition::msgs::Pose_V &_msg,
                      const ignition::transport::MessageInfo &_info)
      {
        // Ignore messages that are published from this bridge.
          this->ign_callback(_msg, ros_pub);
      };

      node->Subscribe(topic_name, subCb);
    }

    BridgeIgnToRosHandles create_bridge_from_ign_to_ros(
      std::shared_ptr<ignition::transport::Node> ign_node,
      ros::NodeHandle ros_node,
      const std::string & ign_topic_name,
      size_t subscriber_queue_size,
      const std::string & ros_topic_name,
      size_t publisher_queue_size)
    {
      ros::Publisher ros_pub = ros_node.advertise<nav_msgs::Odometry>(ros_topic_name, publisher_queue_size);

      this->create_ign_subscriber(ign_node, ign_topic_name, subscriber_queue_size, ros_pub);

      BridgeIgnToRosHandles handles;
      handles.ign_subscriber = ign_node;
      handles.ros_publisher = ros_pub;
      return handles;
    }
};



//////////////////////////////////////////////////
int main(int argc, char * argv[])
{
  if (argc < 2)
  {
    usage();
    return -1;
  }

  // ROS node
  ros::init(argc, argv, "ros_ign_pose_bridge");
  ros::NodeHandle ros_node;

  // Ignition node
  auto ign_node = std::make_shared<ignition::transport::Node>();

  std::list<BridgeIgnToRosHandles> ign_to_ros_handles;

  // Parse all arguments.

  const size_t queue_size = 10;
  for (auto i = 2; i < argc; ++i)
  {
    std::string ign_topic_name = argv[1];
    std::string vehicle_name = std::string(argv[i]);
    std::string ros_topic_name = std::string("/pose/groundtruth/") + vehicle_name;

    Pose_bridge pose_bridge;

    try
    {
        ign_to_ros_handles.push_back(pose_bridge.create_bridge_from_ign_to_ros(
              ign_node, ros_node, ign_topic_name, queue_size, ros_topic_name, queue_size));
    }
    catch (std::runtime_error &_e)
    {
      ROS_ERROR_STREAM("Failed to create a bridge for topic ["
          << vehicle_name << "] "
          << "with ROS topic [" << ros_topic_name << "] and "
          << "Ignition Transport Topic [" << ign_topic_name << "]"
          << std::endl);
    }
  }

  // ROS asynchronous spinner
  ros::AsyncSpinner async_spinner(1);
  async_spinner.start();

  // Zzzzzz.
  ignition::transport::waitForShutdown();

  return 0;
}
